/*******************************************************************************

   SDF.C

   Author: Peter Loan

   Date: 19-JUN-91

   Copyright (c) 1992-5 MusculoGraphics, Inc.
   All rights reserved.
   Portions of this source code are copyrighted by MusculoGraphics, Inc.

   Description: 

   Routines:

*******************************************************************************/

#include "universal.h"
#include "globals.h"
#include "functions.h"
#include "sdfunctions.h"


/*************** DEFINES (for this file only) *********************************/


/*************** STATIC GLOBAL VARIABLES (for this file only) *****************/


/*************** GLOBAL VARIABLES (used in only a few files) ******************/
SDSegment* SDseg = NULL;
int num_SD_segs;
int* joint_order = NULL;


/*************** EXTERNED VARIABLES (declared in another .c file) *************/



/*************** PROTOTYPES for STATIC FUNCTIONS (for this file only) *********/
static void write_loop_joints(FILE* fp, char seg_name[], SegmentSDF* seg, int addQuestionMarks);
static int countConstraints(ModelStruct* ms, int* nq);


/* This function determines much of the information required to make
 * an SD/FAST model from the SIMM model, such as figuring out how to
 * convert each SIMM joint into an SD/FAST joint, and what the degrees
 * of freedom should be. Optionally, it writes out this information to
 * an SD/FAST input file.
 */
ReturnCode make_sdfast_model(ModelStruct* ms, char filename[], SBoolean write_file, int addQuestionMarks)
{
   int i, j, nq, dofcount, constrainedcount, numconstraints, jointnum, dofnum;
   char* time_string;
   char sdf_buffer[CHARBUFFER];
   FILE* fp = NULL;
   JointSDF* jnts;
   DofStruct* dof;
   SegmentSDF* segs;

   /* Make the joint transformation matrices once you have changed the gencoords */

   for (i=0; i<ms->numjoints; i++)
      make_conversion(ms, i);

   /* Re-initialize the SD/FAST variables. Mark all dofs as
    * constrained (not true degrees of freedom).
    * Then for each gencoord, find the ONE dof which best
    * corresponds to the gencoord and mark it unconstrained.
    * That is, of all the dofs that are functions of a particular
    * gencoord, one should be a direct mapping (between dof and
    * gencoord) and the others should be non-trivial functions.
    */

   for (i=0; i<ms->numjoints; i++)
   {
      for (j=0; j<6; j++)
      {
         ms->joint[i].dofs[j].sd.name = NULL;
         ms->joint[i].dofs[j].sd.con_name = NULL;
         ms->joint[i].dofs[j].sd.initial_value = 0.0;
         ms->joint[i].dofs[j].sd.constrained = yes;
         ms->joint[i].dofs[j].sd.fixed = no;
         ms->joint[i].dofs[j].sd.state_number = -1;
         ms->joint[i].dofs[j].sd.error_number = -1;
         ms->joint[i].dofs[j].sd.joint = -1;
         ms->joint[i].dofs[j].sd.axis = -1;
         ms->joint[i].dofs[j].sd.conversion = 0.0;
         ms->joint[i].dofs[j].sd.conversion_sign = 1.0;
      }
   }

   for (i=0; i<ms->numgencoords; i++)
   {
      if (ms->gencoord[i]->used_in_model == yes)
      {
          if (!mark_unconstrained_dof(ms, ms->gencoord[i], &jointnum, &dofnum, NULL))
         {
             sprintf(errorbuffer, "At least one DOF must be a \"simple\" function of gencoord %s (2 points, slope=1, passes thru zero).",
               ms->gencoord[i]->name);
            return code_bad;
         }
      }
   }

   /* Now give the dofs names for use in the SD/FAST code.
    * Names of unconstrained dofs will simply be the names of
    * the gencoords to which they correspond. Names of
    * constrained dofs will be formed from the joint name
    * and the dof keyword (e.g. "hip_tx" and "knee_r1").
    */

   name_dofs(ms);

   FREE_IFNOTNULL(joint_order);
   joint_order = (int*)malloc(ms->numjoints*sizeof(int));
   jnts = (JointSDF*)simm_calloc(ms->numjoints, sizeof(JointSDF));
   segs = (SegmentSDF*)simm_calloc(ms->numsegments, sizeof(SegmentSDF));

   for (i = 0; i < ms->numjoints; i++)
      joint_order[i] = -1;

   find_sdfast_joint_order(ms, jnts, segs, joint_order, SDFAST_GROUND);

   /* Open the SD/FAST input file */

   if (write_file == yes)
   {
      fp = simm_fopen(filename,"w");

      if (fp == NULL)
      {
         sprintf(errorbuffer,"Error creating sdfast file %s", filename);
         error(abort_action,errorbuffer);
         return code_bad;
      }

      make_time_string(&time_string);

      fprintf(fp,"# SD/FAST input file generated by %s\n", program_with_version);
      fprintf(fp,"# Created %s\n", time_string);
      fprintf(fp,"# Name of original SIMM joints file: %s\n#\n\n",
          ms->jointfilename);

      fprintf(fp,"language = c\n\n");

      if (ms->gravity == smX)
         fprintf(fp,"gravity = 9.80665? 0.0? 0.0?\n\n");
      else if (ms->gravity == smNegX)
         fprintf(fp,"gravity = -9.80665? 0.0? 0.0?\n\n");
      else if (ms->gravity == smY)
         fprintf(fp,"gravity = 0.0? 9.80665? 0.0?\n\n");
      else if (ms->gravity == smNegY)
         fprintf(fp,"gravity = 0.0? -9.80665? 0.0?\n\n");
      else if (ms->gravity == smZ)
         fprintf(fp,"gravity = 0.0? 0.0? 9.80665?\n\n");
      else if (ms->gravity == smNegZ)
         fprintf(fp,"gravity = 0.0? 0.0? -9.80665?\n\n");
      else if (ms->gravity == smNoAlign)
         fprintf(fp,"gravity = 0.0? 0.0? 0.0?\n\n");
      else // -Y is the default
         fprintf(fp,"gravity = 0.0? -9.80665? 0.0?\n\n");
   }

   /* Malloc space for the array of segment names. These names include $ground
    * and the names of the "split" body segments. There can be at most
    * (numjoints + 1) segment names.
    */

   /* Free the SDseg array, if it was used previously. */
   if (SDseg)
   {
      for (i = 0; i < num_SD_segs; i++)
         FREE_IFNOTNULL(SDseg[i].name);
      FREE_IFNOTNULL(SDseg);
   }

   SDseg = (SDSegment*)simm_calloc(ms->numjoints + 1, sizeof(SDSegment));
   SDseg[0].name = (char*)simm_malloc(strlen(ms->segment[ms->ground_segment].name) + 2);
   strcpy(SDseg[0].name, ms->segment[ms->ground_segment].name);
   convert_string(SDseg[0].name, yes);
   SDseg[0].simm_segment = ms->ground_segment;
   SDseg[0].mass_center[0] = ms->segment[ms->ground_segment].masscenter[0];
   SDseg[0].mass_center[1] = ms->segment[ms->ground_segment].masscenter[1];
   SDseg[0].mass_center[2] = ms->segment[ms->ground_segment].masscenter[2];
   SDseg[0].mass = 0.0;
   for (i = 0; i < 3; i++)
      for (j = 0; j < 3; j++)
         SDseg[0].inertia[i][j] = 0.0;

   num_SD_segs = 1;

   for (i=0, dofcount=0, constrainedcount=0; i<ms->numjoints; i++)
   {
      if (joint_order[i] < 0)
         continue;
      if (ms->joint[joint_order[i]].type != dpSkippable)
      {
         JointSDF* foo1 = &jnts[joint_order[i]];
         int foo2 = joint_order[i];
         int foo3 = ms->joint[i].sd_num;
         make_sdfast_joint(ms, fp, &jnts[joint_order[i]], joint_order[i], ms->joint[i].sd_num,segs, &dofcount,
                           &constrainedcount, write_file, addQuestionMarks);
      }
   }

   if (write_file == yes)
   {
      for (i=0; i<ms->numsegments; i++)
      {
         if (segs[i].times_split > 0)
         {
            fprintf(fp,"# Loop Joints\n\n");
            break;
         }
      }

      for (i=0; i<ms->numsegments; i++)
      {
         if (segs[i].times_split > 0)
            write_loop_joints(fp, ms->segment[i].name, &segs[i], addQuestionMarks);
      }
   }

   for (i=0; i<ms->numjoints; i++)
   {
      FREE_IFNOTNULL(jnts[i].inbname);
      FREE_IFNOTNULL(jnts[i].outbname);
   }
   free(jnts);
   free(segs);

   /* count the number of constraint functions, which is the number of q's
    * minus the number of SIMM gencoords.
    */

   if (write_file == yes)
   {
      numconstraints = countConstraints(ms, &nq);

      if (numconstraints > 0)
         fprintf(fp,"\n#constraints = %d\n\n", numconstraints);

      for (i=0; i<nq; i++)
      {
         dof = find_nth_q_dof(ms, i);
         if (dof != NULL && dof->sd.constrained == yes)
            fprintf(fp,"constraint = %s \n", dof->sd.con_name);
      }
      for (i = 0; i < ms->num_constraint_objects; i++)
      {
         for (j = 0; j < ms->constraintobj[i].numPoints; j++)
         {
            fprintf(fp, "constraint = %s_", ms->constraintobj[i].name);
            fprintf(fp, "%s\n", ms->constraintobj[i].points[j].name);
         }
      }
      fclose(fp);

      (void)sprintf(sdf_buffer,"Wrote SD/FAST input file %s", filename);
      message(sdf_buffer,0,DEFAULT_MESSAGE_X_OFFSET);
   }

   return code_fine;

}


static int countConstraints(ModelStruct* ms, int* nq)
{
   int i, j, numconstraints;

   /* DKB added sd.fixed = yes otherwise can miss constraint if it is the last q in list */
   for (i = 0, *nq = 0; i < ms->numjoints; i++)
      for (j = 0; j < 6; j++)
         if (ms->joint[i].dofs[j].type == function_dof || ms->joint[i].dofs[j].sd.fixed == yes)
            (*nq)++;

   numconstraints = *nq - (ms->numgencoords - ms->numunusedgencoords);

   for (i = 0; i < ms->num_constraint_objects; i++)
      numconstraints += ms->constraintobj[i].numPoints;

   return numconstraints;

}


char* make_sdfast_seg_name(char seg_name[], int times_split)
{

   int i, new_len, prepend = 0;
   char* new_name;

   if (seg_name[0] >= '0' && seg_name[0] <= '9')
      prepend = 1;

   new_len = prepend + STRLEN(seg_name) + times_split;

   new_name = (char*)malloc(new_len * sizeof(char));

   if (prepend)
   {
      new_name[0] = '_';
      strcpy(&new_name[1], seg_name);
   }
   else
   {
      strcpy(new_name, seg_name);
   }

   for (i = new_len - 2; i > new_len - 2 - times_split; i--)
      new_name[i] = 'p';

   new_name[new_len - 1] = STRING_TERMINATOR;

   return new_name;
}


void make_sdfast_joint(ModelStruct* ms, FILE* fp, JointSDF* jntsdf, int jointnum, int sdnum,
                          SegmentSDF segs[], int* dofcount,
                          int* constrainedcount, SBoolean write_file, int addQuestionMarks)
{
   int i, ax, df, axisnum, axisname[3];
   double axis[3], bodytojoint[3], inbtojoint[3];
   SegmentStruct* seg1;
   SegmentStruct* seg2;
   SegmentSDF* segsdf;
   JointStruct* jnt;
   dpJointType type;
   char prescribed_string[20];
   static char yesQuestionMark[] = "?";
   static char noQuestionMark[] = "";
   char* questionMark;

   if (addQuestionMarks)
       questionMark = yesQuestionMark;
   else
       questionMark = noQuestionMark;

   jnt = &ms->joint[jointnum];
   type = ms->joint[jointnum].type;

   if (jntsdf->dir == FORWARD)
   {
      seg1 = &ms->segment[jnt->from];
      seg2 = &ms->segment[jnt->to];
      segsdf = &segs[jnt->to];
   }
   else
   {
      seg1 = &ms->segment[jnt->to];
      seg2 = &ms->segment[jnt->from];
      segsdf = &segs[jnt->from];
   }

   if (write_file == yes)
   {
      fprintf(fp, "body = %s inb = %s joint = %s\n", jntsdf->outbname,
         jntsdf->inbname, get_joint_type_name(type,jntsdf->dir));
      if (type == dpWeld)
         fprintf(fp, "# this is really a weld joint implemented as a prescribed pin\n");
   }

   /* Add the body name to the list of segment names. This list contains 'real'
    * segment names as well as the names of 'split' body segments.
    */

   mstrcpy(&(SDseg[num_SD_segs].name),jntsdf->outbname);

   /* If there are loops in the model, then SIMM segments get split and there
    * will be more SD segments than SIMM segments. So that unsplittable segment
    * parameters (like contact objects) can be assigned to the SD segments,
    * each SD segment has an index of its corresponding SIMM segment. But for
    * segments that were split, only one piece will have a valid index.
    */
   if (jntsdf->closes_loop == no)
   {
      if (jntsdf->dir == FORWARD)
         SDseg[num_SD_segs].simm_segment = jnt->to;
      else
         SDseg[num_SD_segs].simm_segment = jnt->from;
   }
   else
   {
      SDseg[num_SD_segs].simm_segment = -1;
   }

   SDseg[num_SD_segs].mass = seg2->mass / segsdf->mass_factor;
   SDseg[num_SD_segs].inertia[0][0] = seg2->inertia[0][0] / segsdf->mass_factor;
   SDseg[num_SD_segs].inertia[0][1] = seg2->inertia[0][1] / segsdf->mass_factor;
   SDseg[num_SD_segs].inertia[0][2] = seg2->inertia[0][2] / segsdf->mass_factor;
   SDseg[num_SD_segs].inertia[1][0] = seg2->inertia[1][0] / segsdf->mass_factor;
   SDseg[num_SD_segs].inertia[1][1] = seg2->inertia[1][1] / segsdf->mass_factor;
   SDseg[num_SD_segs].inertia[1][2] = seg2->inertia[1][2] / segsdf->mass_factor;
   SDseg[num_SD_segs].inertia[2][0] = seg2->inertia[2][0] / segsdf->mass_factor;
   SDseg[num_SD_segs].inertia[2][1] = seg2->inertia[2][1] / segsdf->mass_factor;
   SDseg[num_SD_segs].inertia[2][2] = seg2->inertia[2][2] / segsdf->mass_factor;
   SDseg[num_SD_segs].mass_center[0] = seg2->masscenter[0];
   SDseg[num_SD_segs].mass_center[1] = seg2->masscenter[1];
   SDseg[num_SD_segs].mass_center[2] = seg2->masscenter[2];

   if (write_file == yes)
   {
      fprintf(fp, "mass = %20.16lf%s      inertia = %.16lf%s %.16lf%s %.16lf%s\n",
          SDseg[num_SD_segs].mass, questionMark,
          SDseg[num_SD_segs].inertia[0][0], questionMark,
          SDseg[num_SD_segs].inertia[0][1], questionMark,
          SDseg[num_SD_segs].inertia[0][2], questionMark);
      fprintf(fp, "                                    %.16lf%s %.16lf%s %.16lf%s\n",
          SDseg[num_SD_segs].inertia[1][0], questionMark,
          SDseg[num_SD_segs].inertia[1][1], questionMark,
          SDseg[num_SD_segs].inertia[1][2], questionMark);
      fprintf(fp, "                                    %.16lf%s %.16lf%s %.16lf%s\n",
          SDseg[num_SD_segs].inertia[2][0], questionMark,
          SDseg[num_SD_segs].inertia[2][1], questionMark,
          SDseg[num_SD_segs].inertia[2][2], questionMark);
   }

   /* The bodytojoint vector (inbtojoint vector for INVERSE joints) is always
    * defined by the negative of the mass center vector. The inbtojoint vector
    * (bodytojoint for INVERSE joints) can change depending on whether or not
    * some of the translations in the joint are functions. For all translation
    * components that are functions, you want to use just the mass center
    * vector component so that the DOF value is the same as the SIMM gencoord
    * or SIMM constraint function. For components that are constants, you want
    * to add the DOF value to the mass center vector so that the origin ends
    * up in the right place. 4/3/97.
    * In general, the above method works only for joints in which the translations
    * occur before the rotations. For cases in which the translations occur
    * between two or more rotations, the joint cannot be modeled easily in
    * SD/FAST. For cases in which the translations are after the rotations,
    * SD/FAST has "reverse" joints (e.g., rbushing) that automatically handle
    * the translations properly. Rplanar correctly handles this case because
    * the rplanar joint itself handles two of the translations, and the third
    * one is along the axis of rotation, so it does not need to be handled
    * separately. For joints which do not have a reverse (e.g., pin, cylinder,
    * universal, gimbal), you need to attach the translations to the "other"
    * body segment than you normally would. 4/10/97.
    */

   if (type == dpReversePin || type == dpReverseGimbal ||
       type == dpReverseUniversal || type == dpReverseCylindrical)
   {
      if (jntsdf->dir == FORWARD)
      {
         if (jnt->dofs[TX].type == constant_dof)
            bodytojoint[0] = -jnt->dofs[TX].value - seg2->masscenter[0];
         else
            bodytojoint[0] = -seg2->masscenter[0];
         if (jnt->dofs[TY].type == constant_dof)
            bodytojoint[1] = -jnt->dofs[TY].value - seg2->masscenter[1];
         else
            bodytojoint[1] = -seg2->masscenter[1];
         if (jnt->dofs[TZ].type == constant_dof)
            bodytojoint[2] = -jnt->dofs[TZ].value - seg2->masscenter[2];
         else
            bodytojoint[2] = -seg2->masscenter[2];
         inbtojoint[0] = -seg1->masscenter[0];
         inbtojoint[1] = -seg1->masscenter[1];
         inbtojoint[2] = -seg1->masscenter[2];
      }
      else
      {
         bodytojoint[0] = -seg2->masscenter[0];
         bodytojoint[1] = -seg2->masscenter[1];
         bodytojoint[2] = -seg2->masscenter[2];
         if (jnt->dofs[TX].type == constant_dof)
            inbtojoint[0] = -jnt->dofs[TX].value - seg1->masscenter[0];
         else
            inbtojoint[0] = -seg1->masscenter[0];
         if (jnt->dofs[TY].type == constant_dof)
            inbtojoint[1] = -jnt->dofs[TY].value - seg1->masscenter[1];
         else
            inbtojoint[1] = -seg1->masscenter[1];
         if (jnt->dofs[TZ].type == constant_dof)
            inbtojoint[2] = -jnt->dofs[TZ].value - seg1->masscenter[2];
         else
            inbtojoint[2] = -seg1->masscenter[2];
      }
   }
   else
   {
      if (jntsdf->dir == FORWARD)
      {
         bodytojoint[0] = -seg2->masscenter[0];
         bodytojoint[1] = -seg2->masscenter[1];
         bodytojoint[2] = -seg2->masscenter[2];
         if (jnt->dofs[TX].type == constant_dof)
            inbtojoint[0] = jnt->dofs[TX].value - seg1->masscenter[0];
         else
            inbtojoint[0] = -seg1->masscenter[0];
         if (jnt->dofs[TY].type == constant_dof)
            inbtojoint[1] = jnt->dofs[TY].value - seg1->masscenter[1];
         else
            inbtojoint[1] = -seg1->masscenter[1];
         if (jnt->dofs[TZ].type == constant_dof)
            inbtojoint[2] = jnt->dofs[TZ].value - seg1->masscenter[2];
         else
            inbtojoint[2] = -seg1->masscenter[2];
      }
      else
      {
         if (jnt->dofs[TX].type == constant_dof)
            bodytojoint[0] = jnt->dofs[TX].value - seg2->masscenter[0];
         else
            bodytojoint[0] = -seg2->masscenter[0];
         if (jnt->dofs[TY].type == constant_dof)
            bodytojoint[1] = jnt->dofs[TY].value - seg2->masscenter[1];
         else
            bodytojoint[1] = -seg2->masscenter[1];
         if (jnt->dofs[TZ].type == constant_dof)
            bodytojoint[2] = jnt->dofs[TZ].value - seg2->masscenter[2];
         else
            bodytojoint[2] = -seg2->masscenter[2];
         inbtojoint[0] = -seg1->masscenter[0];
         inbtojoint[1] = -seg1->masscenter[1];
         inbtojoint[2] = -seg1->masscenter[2];
      }
   }

   /* Store the vectors in the SDseg struct so they can be
    * passed to the simulation DLL later.
    */
   for (i = 0; i < 3; i++)
   {
      SDseg[num_SD_segs].body_to_joint[i] = bodytojoint[i];
      SDseg[num_SD_segs].inboard_to_joint[i] = inbtojoint[i];
   }

   if (write_file == yes)
   {
      fprintf(fp, "bodytojoint = %.16lf%s %.16lf%s %.16lf%s      inbtojoint = %.16lf%s %.16lf%s %.16lf%s\n",
         bodytojoint[0], questionMark, bodytojoint[1], questionMark, bodytojoint[2], questionMark,
         inbtojoint[0], questionMark, inbtojoint[1], questionMark, inbtojoint[2], questionMark);
   }

   num_SD_segs++;

   if (type == dpWeld)
   {
      if (write_file == yes)
      {
         fprintf(fp, "pin = 1.0%s 0.0%s 0.0%s\n", questionMark, questionMark, questionMark);
         fprintf(fp, "prescribed = 1\n");
      }
      sprintf(buffer,"fixed%d", *dofcount);
      mstrcpy(&jnt->dofs[R1].sd.name,buffer);
      jnt->dofs[R1].sd.state_number = (*dofcount)++;
      jnt->dofs[R1].sd.constrained = no;
      jnt->dofs[R1].sd.fixed = yes;
      jnt->dofs[R1].sd.conversion = RTOD*jnt->dofs[R1].sd.conversion_sign;
      jnt->dofs[R1].sd.joint = sdnum;//jointnum;
      jnt->dofs[R1].sd.axis = 0;
      jnt->dofs[R1].sd.initial_value = 0.0;
   }
   else if (type == dpPin || type == dpReversePin)
   {
      df = find_rotation_axis(jnt, axis);
      if (jntsdf->dir == INVERSE)
      {
         axis[0] = -axis[0];
         axis[1] = -axis[1];
         axis[2] = -axis[2];
      }
      if (write_file == yes)
         fprintf(fp, "pin = %.16lf%s %.16lf%s %.16lf%s\n", axis[0], questionMark, axis[1], questionMark, axis[2], questionMark);
      if (jnt->dofs[df].type == constant_dof)
      {
         if (write_file == yes)
            fprintf(fp, "prescribed = 1\n");
         sprintf(buffer,"fixed%d", *dofcount);
         mstrcpy(&jnt->dofs[df].sd.name,buffer);
         jnt->dofs[df].sd.state_number = (*dofcount)++;
         jnt->dofs[df].sd.constrained = no;
         jnt->dofs[df].sd.fixed = yes;
         jnt->dofs[df].sd.conversion = RTOD*jnt->dofs[df].sd.conversion_sign;
         jnt->dofs[df].sd.joint = sdnum;//jointnum;
         jnt->dofs[df].sd.axis = 0;
         jnt->dofs[df].sd.initial_value = jnt->dofs[df].value;
      }
      else
      {
         jnt->dofs[df].sd.state_number = (*dofcount)++;
         jnt->dofs[df].sd.fixed = no;
         if (jnt->dofs[df].sd.constrained == yes)
            jnt->dofs[df].sd.error_number = (*constrainedcount)++;
         else if (write_file == yes)
            fprintf(fp, "prescribed = 0?\n");
         jnt->dofs[df].sd.conversion = RTOD*jnt->dofs[df].sd.conversion_sign;
         jnt->dofs[df].sd.joint = sdnum;//jointnum;
         jnt->dofs[df].sd.axis = 0;
         if (jnt->dofs[df].sd.constrained == no)
            jnt->dofs[df].sd.initial_value = jnt->dofs[df].gencoord->value;
         else
            jnt->dofs[df].sd.initial_value = jnt->dofs[df].value;
      }
   }
   else if (type == dpSlider)
   {
      df = find_translation_axis(jnt, axis, function_dof, 0);
      if (jntsdf->dir == INVERSE)
      {
         axis[0] = -axis[0];
         axis[1] = -axis[1];
         axis[2] = -axis[2];
      }
      if (write_file == yes)
         fprintf(fp, "pin = %.16lf%s %.16lf%s %.16lf%s\n", axis[0], questionMark, axis[1], questionMark, axis[2], questionMark);
      jnt->dofs[df].sd.state_number = (*dofcount)++;
      jnt->dofs[df].sd.fixed = no;
      if (jnt->dofs[df].sd.constrained == yes)
         jnt->dofs[df].sd.error_number = (*constrainedcount)++;
      else if (write_file == yes)
         fprintf(fp, "prescribed = 0?\n");
      jnt->dofs[df].sd.conversion = jnt->dofs[df].sd.conversion_sign;
      jnt->dofs[df].sd.joint = sdnum;//jointnum;
      jnt->dofs[df].sd.axis = 0;
      if (jnt->dofs[df].sd.constrained == no)
         jnt->dofs[df].sd.initial_value = jnt->dofs[df].gencoord->value;
      else
         jnt->dofs[df].sd.initial_value = jnt->dofs[df].value;
   }
   else if (type == dpPlanar)
   {
      axisnum = 0;
      NULLIFY_STRING(prescribed_string);
      if (jntsdf->dir == FORWARD)
      {
         if (jnt->dofs[TX].type == function_dof)
         {
            if (write_file == yes)
               fprintf(fp, "pin = 1.0%s 0.0%s 0.0%s\n", questionMark, questionMark, questionMark);
            jnt->dofs[TX].sd.state_number = (*dofcount)++;
            jnt->dofs[TX].sd.fixed = no;
            if (jnt->dofs[TX].sd.constrained == yes)
            {
               jnt->dofs[TX].sd.error_number = (*constrainedcount)++;
               strcat(prescribed_string," 0");
               jnt->dofs[TX].sd.initial_value = jnt->dofs[TX].value;
            }
            else
            {
               strcat(prescribed_string," 0?");
               jnt->dofs[TX].sd.initial_value = jnt->dofs[TX].value;
            }
            jnt->dofs[TX].sd.conversion = jnt->dofs[TX].sd.conversion_sign;
            jnt->dofs[TX].sd.joint = sdnum;//jointnum;
            jnt->dofs[TX].sd.axis = axisnum++;
         }
         if (jnt->dofs[TY].type == function_dof)
         {
            if (write_file == yes)
               fprintf(fp, "pin = 0.0%s 1.0%s 0.0%s\n", questionMark, questionMark, questionMark);
            jnt->dofs[TY].sd.state_number = (*dofcount)++;
            jnt->dofs[TY].sd.fixed = no;
            if (jnt->dofs[TY].sd.constrained == yes)
            {
               jnt->dofs[TY].sd.error_number = (*constrainedcount)++;
               strcat(prescribed_string," 0");
               jnt->dofs[TY].sd.initial_value = jnt->dofs[TY].value;
            }
            else
            {
               strcat(prescribed_string," 0?");
               jnt->dofs[TY].sd.initial_value = jnt->dofs[TY].value;
            }
            jnt->dofs[TY].sd.conversion = jnt->dofs[TY].sd.conversion_sign;
            jnt->dofs[TY].sd.joint = sdnum;//jointnum;
            jnt->dofs[TY].sd.axis = axisnum++;
         }
         if (jnt->dofs[TZ].type == function_dof)
         {
            if (write_file == yes)
               fprintf(fp, "pin = 0.0%s 0.0%s 1.0%s\n", questionMark, questionMark, questionMark);
            jnt->dofs[TZ].sd.state_number = (*dofcount)++;
            jnt->dofs[TZ].sd.fixed = no;
            if (jnt->dofs[TZ].sd.constrained == yes)
            {
               jnt->dofs[TZ].sd.error_number = (*constrainedcount)++;
               strcat(prescribed_string," 0");
               jnt->dofs[TZ].sd.initial_value = jnt->dofs[TZ].value;
            }
            else
            {
               strcat(prescribed_string," 0?");
               jnt->dofs[TZ].sd.initial_value = jnt->dofs[TZ].value;
            }
            jnt->dofs[TZ].sd.conversion = jnt->dofs[TZ].sd.conversion_sign;
            jnt->dofs[TZ].sd.joint = sdnum;//jointnum;
            jnt->dofs[TZ].sd.axis = axisnum++;
         }
         df = find_rotation_axis(jnt,axis);
         if (write_file == yes)
            fprintf(fp, "pin = %.16lf%s %.16lf%s %.16lf%s\n", axis[0], questionMark, axis[1], questionMark, axis[2], questionMark);
         if (jnt->dofs[df].type == constant_dof)
         {
            strcat(prescribed_string," 1");
            sprintf(buffer,"fixed%d", *dofcount);
            mstrcpy(&jnt->dofs[df].sd.name,buffer);
            jnt->dofs[df].sd.state_number = (*dofcount)++;
            jnt->dofs[df].sd.constrained = no;
            jnt->dofs[df].sd.fixed = yes;
            jnt->dofs[df].sd.conversion = RTOD*jnt->dofs[df].sd.conversion_sign;
            jnt->dofs[df].sd.joint = sdnum;//jointnum;
            jnt->dofs[df].sd.axis = 2;
            jnt->dofs[df].sd.initial_value = jnt->dofs[df].value;
         }
         else
         {
            jnt->dofs[df].sd.state_number = (*dofcount)++;
            jnt->dofs[df].sd.fixed = no;
            if (jnt->dofs[df].sd.constrained == yes)
            {
               jnt->dofs[df].sd.error_number = (*constrainedcount)++;
               strcat(prescribed_string," 0");
            }
            else
               strcat(prescribed_string," 0?");
            jnt->dofs[df].sd.conversion = RTOD*jnt->dofs[df].sd.conversion_sign;
            jnt->dofs[df].sd.joint = sdnum;//jointnum;
            jnt->dofs[df].sd.axis = 2;
            if (jnt->dofs[df].sd.constrained == no)
               jnt->dofs[df].sd.initial_value = jnt->dofs[df].gencoord->value;
            else
               jnt->dofs[df].sd.initial_value = jnt->dofs[df].value;
         }
      }
      else /* direction == INVERSE */
      {
         df = find_rotation_axis(jnt,axis);
         if (write_file == yes)
            fprintf(fp, "pin = %.16lf%s %.16lf%s %.16lf%s\n", -axis[0], questionMark, -axis[1], questionMark, -axis[2], questionMark);
         if (jnt->dofs[df].type == constant_dof)
         {
            strcat(prescribed_string," 1");
            sprintf(buffer,"fixed%d", *dofcount);
            mstrcpy(&jnt->dofs[df].sd.name,buffer);
            jnt->dofs[df].sd.state_number = (*dofcount)++;
            jnt->dofs[df].sd.constrained = no;
            jnt->dofs[df].sd.fixed = yes;
            jnt->dofs[df].sd.conversion = RTOD*jnt->dofs[df].sd.conversion_sign;
            jnt->dofs[df].sd.joint = sdnum;//jointnum;
            jnt->dofs[df].sd.axis = axisnum++;
            jnt->dofs[df].sd.initial_value = jnt->dofs[df].value;
         }
         else
         {
            jnt->dofs[df].sd.state_number = (*dofcount)++;
            jnt->dofs[df].sd.fixed = no;
            if (jnt->dofs[df].sd.constrained == yes)
            {
               jnt->dofs[df].sd.error_number = (*constrainedcount)++;
               strcat(prescribed_string," 0");
            }
            else
               strcat(prescribed_string," 0?");
            jnt->dofs[df].sd.conversion = RTOD*jnt->dofs[df].sd.conversion_sign;
            jnt->dofs[df].sd.joint = sdnum;//jointnum;
            jnt->dofs[df].sd.axis = axisnum++;
            if (jnt->dofs[df].sd.constrained == no)
               jnt->dofs[df].sd.initial_value = jnt->dofs[df].gencoord->value;
            else
               jnt->dofs[df].sd.initial_value = jnt->dofs[df].value;
         }
         if (jnt->dofs[TZ].type == function_dof)
         {
            if (write_file == yes)
               fprintf(fp, "pin = 0.0%s 0.0%s -1.0%s\n", questionMark, questionMark, questionMark);
            jnt->dofs[TZ].sd.state_number = (*dofcount)++;
            jnt->dofs[TZ].sd.fixed = no;
            if (jnt->dofs[TZ].sd.constrained == yes)
            {
               jnt->dofs[TZ].sd.error_number = (*constrainedcount)++;
               strcat(prescribed_string," 0");
               jnt->dofs[TZ].sd.initial_value = jnt->dofs[TZ].value;
            }
            else
            {
               strcat(prescribed_string," 0?");
               jnt->dofs[TZ].sd.initial_value = jnt->dofs[TZ].value;
            }
            jnt->dofs[TZ].sd.conversion = jnt->dofs[TZ].sd.conversion_sign;
            jnt->dofs[TZ].sd.joint = sdnum;//jointnum;
            jnt->dofs[TZ].sd.axis = axisnum++;
         }
         if (jnt->dofs[TY].type == function_dof)
         {
            if (write_file == yes)
               fprintf(fp, "pin = 0.0%s -1.0%s 0.0%s\n", questionMark, questionMark, questionMark);
            jnt->dofs[TY].sd.state_number = (*dofcount)++;
            jnt->dofs[TY].sd.fixed = no;
            if (jnt->dofs[TY].sd.constrained == yes)
            {
               jnt->dofs[TY].sd.error_number = (*constrainedcount)++;
               strcat(prescribed_string," 0");
               jnt->dofs[TY].sd.initial_value = jnt->dofs[TY].value;
            }
            else
            {
               strcat(prescribed_string," 0?");
               jnt->dofs[TY].sd.initial_value = jnt->dofs[TY].value;
            }
            jnt->dofs[TY].sd.conversion = jnt->dofs[TY].sd.conversion_sign;
            jnt->dofs[TY].sd.joint = sdnum;//jointnum;
            jnt->dofs[TY].sd.axis = axisnum++;
         }
         if (jnt->dofs[TX].type == function_dof)
         {
            if (write_file == yes)
               fprintf(fp, "pin = -1.0%s 0.0%s 0.0%s\n", questionMark, questionMark, questionMark);
            jnt->dofs[TX].sd.state_number = (*dofcount)++;
            jnt->dofs[TX].sd.fixed = no;
            if (jnt->dofs[TX].sd.constrained == yes)
            {
               jnt->dofs[TX].sd.error_number = (*constrainedcount)++;
               strcat(prescribed_string," 0");
               jnt->dofs[TX].sd.initial_value = jnt->dofs[TX].value;
            }
            else
            {
               strcat(prescribed_string," 0?");
               jnt->dofs[TX].sd.initial_value = jnt->dofs[TX].value;
            }
            jnt->dofs[TX].sd.conversion = jnt->dofs[TX].sd.conversion_sign;
            jnt->dofs[TX].sd.joint = sdnum;//jointnum;
            jnt->dofs[TX].sd.axis = axisnum++;
         }
      }
      if (write_file == yes)
         fprintf(fp, "prescribed =%s\n", prescribed_string);
   }
   else if (type == dpReversePlanar)
   {
      axisnum = 0;
      NULLIFY_STRING(prescribed_string);
      if (jntsdf->dir == FORWARD)
      {
         df = find_rotation_axis(jnt,axis);
         if (write_file == yes)
            fprintf(fp, "pin = %.16lf%s %.16lf%s %.16lf%s\n", axis[0], questionMark, axis[1], questionMark, axis[2], questionMark);
         if (jnt->dofs[df].type == constant_dof)
         {
            strcat(prescribed_string," 1");
            sprintf(buffer,"fixed%d", *dofcount);
            mstrcpy(&jnt->dofs[df].sd.name,buffer);
            jnt->dofs[df].sd.state_number = (*dofcount)++;
            jnt->dofs[df].sd.constrained = no;
            jnt->dofs[df].sd.fixed = yes;
            jnt->dofs[df].sd.conversion = RTOD*jnt->dofs[df].sd.conversion_sign;
            jnt->dofs[df].sd.joint = sdnum;//jointnum;
            jnt->dofs[df].sd.axis = axisnum++;
            jnt->dofs[df].sd.initial_value = jnt->dofs[df].value;
         }
         else
         {
            jnt->dofs[df].sd.state_number = (*dofcount)++;
            jnt->dofs[df].sd.fixed = no;
            if (jnt->dofs[df].sd.constrained == yes)
            {
               jnt->dofs[df].sd.error_number = (*constrainedcount)++;
               strcat(prescribed_string," 0");
            }
            else
               strcat(prescribed_string," 0?");
            jnt->dofs[df].sd.conversion = RTOD*jnt->dofs[df].sd.conversion_sign;
            jnt->dofs[df].sd.joint = sdnum;//jointnum;
            jnt->dofs[df].sd.axis = axisnum++;
            if (jnt->dofs[df].sd.constrained == no)
               jnt->dofs[df].sd.initial_value = jnt->dofs[df].gencoord->value;
            else
               jnt->dofs[df].sd.initial_value = jnt->dofs[df].value;
         }
         if (jnt->dofs[TZ].type == function_dof)
         {
            if (write_file == yes)
               fprintf(fp, "pin = 0.0%s 0.0%s 1.0%s\n", questionMark, questionMark, questionMark);
            jnt->dofs[TZ].sd.state_number = (*dofcount)++;
            jnt->dofs[TZ].sd.fixed = no;
            if (jnt->dofs[TZ].sd.constrained == yes)
            {
               jnt->dofs[TZ].sd.error_number = (*constrainedcount)++;
               strcat(prescribed_string," 0");
               jnt->dofs[TZ].sd.initial_value = jnt->dofs[TZ].value;
            }
            else
            {
               strcat(prescribed_string," 0?");
               jnt->dofs[TZ].sd.initial_value = jnt->dofs[TZ].value;
            }
            jnt->dofs[TZ].sd.conversion = jnt->dofs[TZ].sd.conversion_sign;
            jnt->dofs[TZ].sd.joint = sdnum;//jointnum;
            jnt->dofs[TZ].sd.axis = axisnum++;
         }
         if (jnt->dofs[TY].type == function_dof)
         {
            if (write_file == yes)
               fprintf(fp, "pin = 0.0%s 1.0%s 0.0%s\n", questionMark, questionMark, questionMark);
            jnt->dofs[TY].sd.state_number = (*dofcount)++;
            jnt->dofs[TY].sd.fixed = no;
            if (jnt->dofs[TY].sd.constrained == yes)
            {
               jnt->dofs[TY].sd.error_number = (*constrainedcount)++;
               strcat(prescribed_string," 0");
               jnt->dofs[TY].sd.initial_value = jnt->dofs[TY].value;
            }
            else
            {
               strcat(prescribed_string," 0?");
               jnt->dofs[TY].sd.initial_value = jnt->dofs[TY].value;
            }
            jnt->dofs[TY].sd.conversion = jnt->dofs[TY].sd.conversion_sign;
            jnt->dofs[TY].sd.joint = sdnum;//jointnum;
            jnt->dofs[TY].sd.axis = axisnum++;
         }
         if (jnt->dofs[TX].type == function_dof)
         {
            if (write_file == yes)
               fprintf(fp, "pin = 1.0%s 0.0%s 0.0%s\n", questionMark, questionMark, questionMark);
            jnt->dofs[TX].sd.state_number = (*dofcount)++;
            jnt->dofs[TX].sd.fixed = no;
            if (jnt->dofs[TX].sd.constrained == yes)
            {
               jnt->dofs[TX].sd.error_number = (*constrainedcount)++;
               strcat(prescribed_string," 0");
               jnt->dofs[TX].sd.initial_value = jnt->dofs[TX].value;
            }
            else
            {
               strcat(prescribed_string," 0?");
               jnt->dofs[TX].sd.initial_value = jnt->dofs[TX].value;
            }
            jnt->dofs[TX].sd.conversion = jnt->dofs[TX].sd.conversion_sign;
            jnt->dofs[TX].sd.joint = sdnum;//jointnum;
            jnt->dofs[TX].sd.axis = axisnum++;
         }
      }
      else /* direction == INVERSE */
      {
         if (jnt->dofs[TX].type == function_dof)
         {
            if (write_file == yes)
               fprintf(fp, "pin = -1.0%s 0.0%s 0.0%s\n", questionMark, questionMark, questionMark);
            jnt->dofs[TX].sd.state_number = (*dofcount)++;
            jnt->dofs[TX].sd.fixed = no;
            if (jnt->dofs[TX].sd.constrained == yes)
            {
               jnt->dofs[TX].sd.error_number = (*constrainedcount)++;
               strcat(prescribed_string," 0");
               jnt->dofs[TX].sd.initial_value = jnt->dofs[TX].value;
            }
            else
            {
               strcat(prescribed_string," 0?");
               jnt->dofs[TX].sd.initial_value = jnt->dofs[TX].value;
            }
            jnt->dofs[TX].sd.conversion = jnt->dofs[TX].sd.conversion_sign;
            jnt->dofs[TX].sd.joint = sdnum;//jointnum;
            jnt->dofs[TX].sd.axis = axisnum++;
         }
         if (jnt->dofs[TY].type == function_dof)
         {
            if (write_file == yes)
               fprintf(fp, "pin = 0.0%s -1.0%s 0.0%s\n", questionMark, questionMark, questionMark);
            jnt->dofs[TY].sd.state_number = (*dofcount)++;
            jnt->dofs[TY].sd.fixed = no;
            if (jnt->dofs[TY].sd.constrained == yes)
            {
               jnt->dofs[TY].sd.error_number = (*constrainedcount)++;
               strcat(prescribed_string," 0");
               jnt->dofs[TY].sd.initial_value = jnt->dofs[TY].value;
            }
            else
            {
               strcat(prescribed_string," 0?");
               jnt->dofs[TY].sd.initial_value = jnt->dofs[TY].value;
            }
            jnt->dofs[TY].sd.conversion = jnt->dofs[TY].sd.conversion_sign;
            jnt->dofs[TY].sd.joint = sdnum;//jointnum;
            jnt->dofs[TY].sd.axis = axisnum++;
         }
         if (jnt->dofs[TZ].type == function_dof)
         {
            if (write_file == yes)
               fprintf(fp, "pin = 0.0%s 0.0%s -1.0%s\n", questionMark, questionMark, questionMark);
            jnt->dofs[TZ].sd.state_number = (*dofcount)++;
            jnt->dofs[TZ].sd.fixed = no;
            if (jnt->dofs[TZ].sd.constrained == yes)
            {
               jnt->dofs[TZ].sd.error_number = (*constrainedcount)++;
               strcat(prescribed_string," 0");
               jnt->dofs[TZ].sd.initial_value = jnt->dofs[TZ].value;
            }
            else
            {
               strcat(prescribed_string," 0?");
               jnt->dofs[TZ].sd.initial_value = jnt->dofs[TZ].value;
            }
            jnt->dofs[TZ].sd.conversion = jnt->dofs[TZ].sd.conversion_sign;
            jnt->dofs[TZ].sd.joint = sdnum;//jointnum;
            jnt->dofs[TZ].sd.axis = axisnum++;
         }
         df = find_rotation_axis(jnt,axis);
         if (write_file == yes)
            fprintf(fp, "pin = %.16lf%s %.16lf%s %.16lf%s\n", -axis[0], questionMark, -axis[1], questionMark, -axis[2], questionMark);
         if (jnt->dofs[df].type == constant_dof)
         {
            strcat(prescribed_string," 1");
            sprintf(buffer,"fixed%d", *dofcount);
            mstrcpy(&jnt->dofs[df].sd.name,buffer);
            jnt->dofs[df].sd.state_number = (*dofcount)++;
            jnt->dofs[df].sd.constrained = no;
            jnt->dofs[df].sd.fixed = yes;
            jnt->dofs[df].sd.conversion = RTOD*jnt->dofs[df].sd.conversion_sign;
            jnt->dofs[df].sd.joint = sdnum;//jointnum;
            jnt->dofs[df].sd.axis = axisnum++;
            jnt->dofs[df].sd.initial_value = jnt->dofs[df].value;
         }
         else
         {
            jnt->dofs[df].sd.state_number = (*dofcount)++;
            jnt->dofs[df].sd.fixed = no;
            if (jnt->dofs[df].sd.constrained == yes)
            {
               jnt->dofs[df].sd.error_number = (*constrainedcount)++;
               strcat(prescribed_string," 0");
            }
            else
               strcat(prescribed_string," 0?");
            jnt->dofs[df].sd.conversion = RTOD*jnt->dofs[df].sd.conversion_sign;
            jnt->dofs[df].sd.joint = sdnum;//jointnum;
            jnt->dofs[df].sd.axis = axisnum++;
            if (jnt->dofs[df].sd.constrained == no)
               jnt->dofs[df].sd.initial_value = jnt->dofs[df].gencoord->value;
            else
               jnt->dofs[df].sd.initial_value = jnt->dofs[df].value;
         }
      }
      if (write_file == yes)
         fprintf(fp, "prescribed =%s\n", prescribed_string);
   }
   else if (type == dpUniversal || type == dpReverseUniversal)
   {
      int axis_count = 0;
      NULLIFY_STRING(prescribed_string);
      for (i=0; i<3; i++)
      {
         if (jntsdf->dir == FORWARD)
            ax = find_nth_rotation(jnt,i+1) - 1;
         else
            ax = find_nth_rotation(jnt,3-i) - 1;
         /* TODO: after enhancing find_rotation_axis(), it should be called here instead
           * of find_nth_rotation(), which doesn't check for zero rotations.
           */
         if (jnt->dofs[ax].type == constant_dof && EQUAL_WITHIN_ERROR(jnt->dofs[ax].value,0.0))
            continue;
         if (write_file == yes)
         {
            if (jntsdf->dir == FORWARD)
            {
               double axis[4];
               COPY_1X4VECTOR(jnt->parentrotaxes[ax], axis);
               normalize_vector(axis, axis);
               fprintf(fp, "pin = %.16lf%s %.16lf%s %.16lf%s\n", axis[0], questionMark, axis[1], questionMark, axis[2], questionMark);
            }
            else
            {
               double axis[4];
               COPY_1X4VECTOR(jnt->parentrotaxes[ax], axis);
               normalize_vector(axis, axis);
               fprintf(fp, "pin = %.16lf%s %.16lf%s %.16lf%s\n", -axis[0], questionMark, -axis[1], questionMark, -axis[2], questionMark);
            }
         }
         if (jnt->dofs[ax].type == constant_dof)
         {
            strcat(prescribed_string," 1");
            sprintf(buffer,"fixed%d", *dofcount);
            mstrcpy(&jnt->dofs[ax].sd.name,buffer);
            jnt->dofs[ax].sd.state_number = (*dofcount)++;
            jnt->dofs[ax].sd.constrained = no;
            jnt->dofs[ax].sd.fixed = yes;
            jnt->dofs[ax].sd.conversion = RTOD*jnt->dofs[ax].sd.conversion_sign;
            jnt->dofs[ax].sd.joint = sdnum;//jointnum;
            jnt->dofs[ax].sd.axis = axis_count++;
            jnt->dofs[ax].sd.initial_value = jnt->dofs[ax].value;
         }
         else
         {
            jnt->dofs[ax].sd.state_number = (*dofcount)++;
            jnt->dofs[ax].sd.fixed = no;
            if (jnt->dofs[ax].sd.constrained == yes)
            {
               jnt->dofs[ax].sd.error_number = (*constrainedcount)++;
               strcat(prescribed_string," 0");
            }
            else
               strcat(prescribed_string," 0?");
            jnt->dofs[ax].sd.conversion = RTOD*jnt->dofs[ax].sd.conversion_sign;
            jnt->dofs[ax].sd.joint = sdnum;//jointnum;
            jnt->dofs[ax].sd.axis = axis_count++;
            if (jnt->dofs[ax].sd.constrained == no)
               jnt->dofs[ax].sd.initial_value = jnt->dofs[ax].gencoord->value;
            else
               jnt->dofs[ax].sd.initial_value = jnt->dofs[ax].value;
         }
      }
      if (write_file == yes)
         fprintf(fp, "prescribed =%s\n", prescribed_string);
   }
   else if (type == dpCylindrical || type == dpReverseCylindrical)
   {
      NULLIFY_STRING(prescribed_string);
      if (jntsdf->dir == FORWARD)
      {
     for (i=TX; i<=TZ; i++)
     {
        if (jnt->dofs[i].type == function_dof)
        {
           jnt->dofs[i].sd.state_number = (*dofcount)++;
           jnt->dofs[i].sd.fixed = no;
           if (jnt->dofs[i].sd.constrained == yes)
           {
          jnt->dofs[i].sd.error_number = (*constrainedcount)++;
          strcat(prescribed_string," 0");
          jnt->dofs[i].sd.initial_value = jnt->dofs[i].value;
           }
           else
           {
          strcat(prescribed_string," 0?");
          jnt->dofs[i].sd.initial_value = jnt->dofs[i].value;
           }
           jnt->dofs[i].sd.conversion = jnt->dofs[i].sd.conversion_sign;
           jnt->dofs[i].sd.joint = sdnum;//jointnum;
           jnt->dofs[i].sd.axis = 0;
           break;
        }
     }
     df = find_rotation_axis(jnt,axis);
     if (write_file == yes)
        fprintf(fp, "pin = %.16lf%s %.16lf%s %.16lf%s\n", axis[0], questionMark, axis[1], questionMark, axis[2], questionMark);
     if (jnt->dofs[df].type == constant_dof)
     {
        strcat(prescribed_string," 1");
        sprintf(buffer,"fixed%d", *dofcount);
        mstrcpy(&jnt->dofs[df].sd.name,buffer);
        jnt->dofs[df].sd.state_number = (*dofcount)++;
        jnt->dofs[df].sd.constrained = no;
        jnt->dofs[df].sd.fixed = yes;
        jnt->dofs[df].sd.conversion = RTOD*jnt->dofs[df].sd.conversion_sign;
        jnt->dofs[df].sd.joint = sdnum;//jointnum;
        jnt->dofs[df].sd.axis = 1;
        jnt->dofs[df].sd.initial_value = jnt->dofs[df].value;
     }
     else
     {
        jnt->dofs[df].sd.state_number = (*dofcount)++;
        jnt->dofs[df].sd.fixed = no;
        if (jnt->dofs[df].sd.constrained == yes)
        {
           jnt->dofs[df].sd.error_number = (*constrainedcount)++;
           strcat(prescribed_string," 0");
        }
        else
           strcat(prescribed_string," 0?");
        jnt->dofs[df].sd.conversion = RTOD*jnt->dofs[df].sd.conversion_sign;
        jnt->dofs[df].sd.joint = sdnum;//jointnum;
        jnt->dofs[df].sd.axis = 1;
        if (jnt->dofs[df].sd.constrained == no)
           jnt->dofs[df].sd.initial_value = jnt->dofs[df].gencoord->value;
        else
           jnt->dofs[df].sd.initial_value = jnt->dofs[df].value;
     }
      }
      else /* direction == INVERSE */
      {
     for (i=TX; i<=TZ; i++)
     {
        if (jnt->dofs[i].type == function_dof)
        {
           jnt->dofs[i].sd.state_number = (*dofcount)++;
           jnt->dofs[i].sd.fixed = no;
           if (jnt->dofs[i].sd.constrained == yes)
           {
          jnt->dofs[i].sd.error_number = (*constrainedcount)++;
          strcat(prescribed_string," 0");
          jnt->dofs[i].sd.initial_value = jnt->dofs[i].value;
           }
           else
           {
          strcat(prescribed_string," 0?");
          jnt->dofs[i].sd.initial_value = jnt->dofs[i].value;
           }
           jnt->dofs[i].sd.conversion = jnt->dofs[i].sd.conversion_sign;
           jnt->dofs[i].sd.joint = sdnum;//jointnum;
           jnt->dofs[i].sd.axis = 0;
           break;
        }
     }
     df = find_rotation_axis(jnt,axis);
     if (write_file == yes)
        fprintf(fp, "pin = %.16lf%s %.16lf%s %.16lf%s\n", -axis[0], questionMark, -axis[1], questionMark, -axis[2], questionMark);
     if (jnt->dofs[df].type == constant_dof)
     {
        strcat(prescribed_string," 1");
        sprintf(buffer,"fixed%d", *dofcount);
        mstrcpy(&jnt->dofs[df].sd.name,buffer);
        jnt->dofs[df].sd.state_number = (*dofcount)++;
        jnt->dofs[df].sd.constrained = no;
        jnt->dofs[df].sd.fixed = yes;
        jnt->dofs[df].sd.conversion = RTOD*jnt->dofs[df].sd.conversion_sign;
        jnt->dofs[df].sd.joint = sdnum;//jointnum;
        jnt->dofs[df].sd.axis = 1;
        jnt->dofs[df].sd.initial_value = jnt->dofs[df].value;
     }
     else
     {
        jnt->dofs[df].sd.state_number = (*dofcount)++;
        jnt->dofs[df].sd.fixed = no;
        if (jnt->dofs[df].sd.constrained == yes)
        {
           jnt->dofs[df].sd.error_number = (*constrainedcount)++;
           strcat(prescribed_string," 0");
        }
        else
           strcat(prescribed_string," 0?");
        jnt->dofs[df].sd.conversion = RTOD*jnt->dofs[df].sd.conversion_sign;
        jnt->dofs[df].sd.joint = sdnum;//jointnum;
        jnt->dofs[df].sd.axis = 1;
        if (jnt->dofs[df].sd.constrained == no)
           jnt->dofs[df].sd.initial_value = jnt->dofs[df].gencoord->value;
        else
           jnt->dofs[df].sd.initial_value = jnt->dofs[df].value;
     }
      }
      if (write_file == yes)
     fprintf(fp, "prescribed =%s\n", prescribed_string);
   }
   else if (type == dpGimbal || type == dpReverseGimbal)
   {
      NULLIFY_STRING(prescribed_string);
      for (i=0; i<3; i++)
      {
         if (jntsdf->dir == FORWARD)
            ax = find_nth_rotation(jnt,i+1) - 1;
         else
            ax = find_nth_rotation(jnt,3-i) - 1;
         if (write_file == yes)
         {
            if (jntsdf->dir == FORWARD)
            {
               double axis[4];
               COPY_1X4VECTOR(jnt->parentrotaxes[ax], axis);
               normalize_vector(axis, axis);
               fprintf(fp, "pin = %.16lf%s %.16lf%s %.16lf%s\n", axis[0], questionMark, axis[1], questionMark, axis[2], questionMark);
            }
            else
            {
               double axis[4];
               COPY_1X4VECTOR(jnt->parentrotaxes[ax], axis);
               normalize_vector(axis, axis);
               fprintf(fp, "pin = %.16lf%s %.16lf%s %.16lf%s\n", -axis[0], questionMark, -axis[1], questionMark, -axis[2], questionMark);
            }
         }
         if (jnt->dofs[ax].type == constant_dof)
         {
            strcat(prescribed_string," 1");
            sprintf(buffer,"fixed%d", *dofcount);
            mstrcpy(&jnt->dofs[ax].sd.name,buffer);
            jnt->dofs[ax].sd.state_number = (*dofcount)++;
            jnt->dofs[ax].sd.constrained = no;
            jnt->dofs[ax].sd.fixed = yes;
            jnt->dofs[ax].sd.conversion = RTOD*jnt->dofs[ax].sd.conversion_sign;
            jnt->dofs[ax].sd.joint = sdnum;//jointnum;
            jnt->dofs[ax].sd.axis = i;
            jnt->dofs[ax].sd.initial_value = jnt->dofs[ax].value;
         }
         else
         {
            jnt->dofs[ax].sd.state_number = (*dofcount)++;
            jnt->dofs[ax].sd.fixed = no;
            if (jnt->dofs[ax].sd.constrained == yes)
            {
               jnt->dofs[ax].sd.error_number = (*constrainedcount)++;
               strcat(prescribed_string," 0");
            }
            else
               strcat(prescribed_string," 0?");
            jnt->dofs[ax].sd.conversion = RTOD*jnt->dofs[ax].sd.conversion_sign;
            jnt->dofs[ax].sd.joint = sdnum;//jointnum;
            jnt->dofs[ax].sd.axis = i;
            if (jnt->dofs[ax].sd.constrained == yes)
               jnt->dofs[ax].sd.initial_value = jnt->dofs[ax].value;
            else
               jnt->dofs[ax].sd.initial_value = jnt->dofs[ax].gencoord->value;
         }
      }
      if (write_file == yes)
         fprintf(fp, "prescribed =%s\n", prescribed_string);
   }
   /* TODO: 4/1/97: bushing joints are defined by the 3 sliding axes (which
    * in SIMM are always the X, Y, and Z axes. If the SIMM rotation axes are
    * not the same, then there's trouble modeling the joint. For now, assume
    * that the rotation axes are the same, and use them to write out axis
    * coordinates, since their order matters.
    */
   else if (type == dpBushing)
   {
      NULLIFY_STRING(prescribed_string);
      axisnum = 0;
      /* Assuming that the rotation axes are the X, Y, and Z axes (see comment
       * above), you want to find out the order of the rotations in advance,
       * so you can process the translations in that order as well, even if they
       * come before the rotations.
       */
      if (jntsdf->dir == FORWARD)
      {
         for (i=0; i<3; i++)
         {
            double axis[4];
            ax = find_nth_rotation(jnt, i+1) - 1;
            COPY_1X4VECTOR(jnt->parentrotaxes[ax], axis);
            normalize_vector(axis, axis);
            if (EQUAL_WITHIN_ERROR(axis[0],1.0))
               axisname[i] = TX;
            else if (EQUAL_WITHIN_ERROR(axis[1],1.0))
               axisname[i] = TY;
            else
               axisname[i] = TZ;
         }
         for (i=0; i<3; i++) /* TX, TY, TZ */
         {
            jnt->dofs[axisname[i]].sd.state_number = (*dofcount)++;
            jnt->dofs[axisname[i]].sd.fixed = no;
            if (jnt->dofs[axisname[i]].sd.constrained == yes)
            {
               jnt->dofs[axisname[i]].sd.error_number = (*constrainedcount)++;
               strcat(prescribed_string," 0");
               jnt->dofs[axisname[i]].sd.initial_value = jnt->dofs[axisname[i]].value;
            }
            else
            {
               strcat(prescribed_string," 0?");
               jnt->dofs[axisname[i]].sd.initial_value = jnt->dofs[axisname[i]].value;
            }
            jnt->dofs[axisname[i]].sd.conversion = jnt->dofs[axisname[i]].sd.conversion_sign;
            jnt->dofs[axisname[i]].sd.joint = sdnum;//jointnum;
            jnt->dofs[axisname[i]].sd.axis = axisnum++;
         }
         for (i=0; i<3; i++) /* R1, R2, R3 */
         {
            double axis[4];
            ax = find_nth_rotation(jnt, i+1) - 1;
            COPY_1X4VECTOR(jnt->parentrotaxes[ax], axis);
            normalize_vector(axis, axis);
            if (write_file == yes)
               fprintf(fp, "pin = %.16lf%s %.16lf%s %.16lf%s\n", axis[0], questionMark, axis[1], questionMark, axis[2], questionMark);
            if (jnt->dofs[ax].type == constant_dof)
            {
               strcat(prescribed_string," 1");
               sprintf(buffer,"fixed%d", *dofcount);
               mstrcpy(&jnt->dofs[ax].sd.name,buffer);
               jnt->dofs[ax].sd.state_number = (*dofcount)++;
               jnt->dofs[ax].sd.constrained = no;
               jnt->dofs[ax].sd.fixed = yes;
               jnt->dofs[ax].sd.conversion = RTOD*jnt->dofs[ax].sd.conversion_sign;
               jnt->dofs[ax].sd.joint = sdnum;//jointnum;
               jnt->dofs[ax].sd.axis = axisnum++;
               jnt->dofs[ax].sd.initial_value = jnt->dofs[ax].value;
            }
            else
            {
               jnt->dofs[ax].sd.state_number = (*dofcount)++;
               jnt->dofs[ax].sd.fixed = no;
               if (jnt->dofs[ax].sd.constrained == yes)
               {
                  jnt->dofs[ax].sd.error_number = (*constrainedcount)++;
                  strcat(prescribed_string," 0");
               }
               else
                  strcat(prescribed_string," 0?");
               jnt->dofs[ax].sd.conversion = RTOD*jnt->dofs[ax].sd.conversion_sign;
               jnt->dofs[ax].sd.joint = sdnum;//jointnum;
               jnt->dofs[ax].sd.axis = axisnum++;
               if (jnt->dofs[ax].sd.constrained == yes)
                  jnt->dofs[ax].sd.initial_value = jnt->dofs[ax].value;
               else
                  jnt->dofs[ax].sd.initial_value = jnt->dofs[ax].gencoord->value;
            }
         }
      }
      else /* direction == INVERSE */
      {
         for (i=0; i<3; i++)
         {
            double axis[4];
            ax = find_nth_rotation(jnt, 3-i) - 1;
            COPY_1X4VECTOR(jnt->parentrotaxes[ax], axis);
            normalize_vector(axis, axis);
            if (EQUAL_WITHIN_ERROR(axis[0],1.0))
               axisname[i] = TX;
            else if (EQUAL_WITHIN_ERROR(axis[1],1.0))
               axisname[i] = TY;
            else
               axisname[i] = TZ;
         }
         for (i=0; i<3; i++)
         {
            double axis[4];
            ax = find_nth_rotation(jnt, 3-i) - 1;
            COPY_1X4VECTOR(jnt->parentrotaxes[ax], axis);
            normalize_vector(axis, axis);
            if (write_file == yes)
               fprintf(fp, "pin = %.16lf%s %.16lf%s %.16lf%s\n", -axis[0], questionMark, -axis[1], questionMark, -axis[2], questionMark);
            if (jnt->dofs[ax].type == constant_dof)
            {
               strcat(prescribed_string," 1");
               sprintf(buffer,"fixed%d", *dofcount);
               mstrcpy(&jnt->dofs[ax].sd.name,buffer);
               jnt->dofs[ax].sd.state_number = (*dofcount)++;
               jnt->dofs[ax].sd.constrained = no;
               jnt->dofs[ax].sd.fixed = yes;
               jnt->dofs[ax].sd.conversion = RTOD*jnt->dofs[ax].sd.conversion_sign;
               jnt->dofs[ax].sd.joint = sdnum;//jointnum;
               jnt->dofs[ax].sd.axis = axisnum++;
               jnt->dofs[ax].sd.initial_value = jnt->dofs[ax].value;
            }
            else
            {
               jnt->dofs[ax].sd.state_number = (*dofcount)++;
               jnt->dofs[ax].sd.fixed = no;
               if (jnt->dofs[ax].sd.constrained == yes)
               {
                  jnt->dofs[ax].sd.error_number = (*constrainedcount)++;
                  strcat(prescribed_string," 0");
               }
               else
                  strcat(prescribed_string," 0?");
               jnt->dofs[ax].sd.conversion = RTOD*jnt->dofs[ax].sd.conversion_sign;
               jnt->dofs[ax].sd.joint = sdnum;//jointnum;
               jnt->dofs[ax].sd.axis = axisnum++;
               if (jnt->dofs[ax].sd.constrained == yes)
                  jnt->dofs[ax].sd.initial_value = jnt->dofs[ax].value;
               else
                  jnt->dofs[ax].sd.initial_value = jnt->dofs[ax].gencoord->value;
            }
         }
         for (i=0; i<3; i++)   /* TX, TY, TZ */
         {
            jnt->dofs[axisname[i]].sd.state_number = (*dofcount)++;
            jnt->dofs[axisname[i]].sd.fixed = no;
            if (jnt->dofs[axisname[i]].sd.constrained == yes)
            {
               jnt->dofs[axisname[i]].sd.error_number = (*constrainedcount)++;
               strcat(prescribed_string," 0");
               jnt->dofs[axisname[i]].sd.initial_value = jnt->dofs[axisname[i]].value;
            }
            else
            {
               strcat(prescribed_string," 0?");
               jnt->dofs[axisname[i]].sd.initial_value = jnt->dofs[axisname[i]].value;
            }
            jnt->dofs[axisname[i]].sd.conversion = jnt->dofs[axisname[i]].sd.conversion_sign;
            jnt->dofs[axisname[i]].sd.joint = sdnum;//jointnum;
            jnt->dofs[axisname[i]].sd.axis = axisnum++;
         }
      }
      if (write_file == yes)
         fprintf(fp, "prescribed =%s\n", prescribed_string);
   }
   else if (type == dpReverseBushing)
   {
      NULLIFY_STRING(prescribed_string);
      axisnum = 0;
      if (jntsdf->dir == FORWARD)
      {
         for (i=0; i<3; i++)
         {
            double axis[4];
            ax = find_nth_rotation(jnt, i+1) - 1;
            COPY_1X4VECTOR(jnt->parentrotaxes[ax], axis);
            normalize_vector(axis, axis);
            if (EQUAL_WITHIN_ERROR(axis[0],1.0))
               axisname[i] = TX;
            else if (EQUAL_WITHIN_ERROR(axis[1],1.0))
               axisname[i] = TY;
            else
               axisname[i] = TZ;
         }
         for (i=0; i<3; i++)
         {
            double axis[4];
            ax = find_nth_rotation(jnt, i+1) - 1;
            COPY_1X4VECTOR(jnt->parentrotaxes[ax], axis);
            normalize_vector(axis, axis);
            if (write_file == yes)
               fprintf(fp, "pin = %.16lf%s %.16lf%s %.16lf%s\n", axis[0], questionMark, axis[1], questionMark, axis[2], questionMark);
            if (jnt->dofs[ax].type == constant_dof)
            {
               strcat(prescribed_string," 1");
               sprintf(buffer,"fixed%d", *dofcount);
               mstrcpy(&jnt->dofs[ax].sd.name,buffer);
               jnt->dofs[ax].sd.state_number = (*dofcount)++;
               jnt->dofs[ax].sd.constrained = no;
               jnt->dofs[ax].sd.fixed = yes;
               jnt->dofs[ax].sd.conversion = RTOD*jnt->dofs[ax].sd.conversion_sign;
               jnt->dofs[ax].sd.joint = sdnum;//jointnum;
               jnt->dofs[ax].sd.axis = axisnum++;
               jnt->dofs[ax].sd.initial_value = jnt->dofs[ax].value;
            }
            else
            {
               jnt->dofs[ax].sd.state_number = (*dofcount)++;
               jnt->dofs[ax].sd.fixed = no;
               if (jnt->dofs[ax].sd.constrained == yes)
               {
                  jnt->dofs[ax].sd.error_number = (*constrainedcount)++;
                  strcat(prescribed_string," 0");
               }
               else
                  strcat(prescribed_string," 0?");
               jnt->dofs[ax].sd.conversion = RTOD*jnt->dofs[ax].sd.conversion_sign;
               jnt->dofs[ax].sd.joint = sdnum;//jointnum;
               jnt->dofs[ax].sd.axis = axisnum++;
               if (jnt->dofs[ax].sd.constrained == yes)
                  jnt->dofs[ax].sd.initial_value = jnt->dofs[ax].value;
               else
                  jnt->dofs[ax].sd.initial_value = jnt->dofs[ax].gencoord->value;
            }
         }
         for (i=0; i<3; i++) /* TX, TY, TZ */
         {
            jnt->dofs[axisname[i]].sd.state_number = (*dofcount)++;
            jnt->dofs[axisname[i]].sd.fixed = no;
            if (jnt->dofs[axisname[i]].sd.constrained == yes)
            {
               jnt->dofs[axisname[i]].sd.error_number = (*constrainedcount)++;
               strcat(prescribed_string," 0");
               jnt->dofs[axisname[i]].sd.initial_value = jnt->dofs[axisname[i]].value;
            }
            else
            {
               strcat(prescribed_string," 0?");
               jnt->dofs[axisname[i]].sd.initial_value = jnt->dofs[axisname[i]].value;
            }
            jnt->dofs[axisname[i]].sd.conversion = jnt->dofs[axisname[i]].sd.conversion_sign;
            jnt->dofs[axisname[i]].sd.joint = sdnum;//jointnum;
            jnt->dofs[axisname[i]].sd.axis = axisnum++;
         }
      }
      else
      {
         for (i=0; i<3; i++)
         {
            double axis[4];
            ax = find_nth_rotation(jnt, 3-i) - 1;
            COPY_1X4VECTOR(jnt->parentrotaxes[ax], axis);
            normalize_vector(axis, axis);
            if (EQUAL_WITHIN_ERROR(axis[0],1.0))
               axisname[i] = TX;
            else if (EQUAL_WITHIN_ERROR(axis[1],1.0))
               axisname[i] = TY;
            else
               axisname[i] = TZ;
         }
         for (i=0; i<3; i++) /* TX, TY, TZ */
         {
            jnt->dofs[axisname[i]].sd.state_number = (*dofcount)++;
            jnt->dofs[axisname[i]].sd.fixed = no;
            if (jnt->dofs[axisname[i]].sd.constrained == yes)
            {
               jnt->dofs[axisname[i]].sd.error_number = (*constrainedcount)++;
               strcat(prescribed_string," 0");
               jnt->dofs[axisname[i]].sd.initial_value = jnt->dofs[axisname[i]].value;
            }
            else
            {
               strcat(prescribed_string," 0?");
               jnt->dofs[axisname[i]].sd.initial_value = jnt->dofs[axisname[i]].value;
            }
            jnt->dofs[axisname[i]].sd.conversion = jnt->dofs[axisname[i]].sd.conversion_sign;
            jnt->dofs[axisname[i]].sd.joint = sdnum;//jointnum;
            jnt->dofs[axisname[i]].sd.axis = axisnum++;
         }
         for (i=0; i<3; i++)
         {
            double axis[4];
            ax = find_nth_rotation(jnt, 3-i) - 1;
            COPY_1X4VECTOR(jnt->parentrotaxes[ax], axis);
            normalize_vector(axis, axis);
            if (write_file == yes)
               fprintf(fp, "pin = %.16lf%s %.16lf%s %.16lf%s\n", -axis[0], questionMark, -axis[1], questionMark, -axis[2], questionMark);
            if (jnt->dofs[ax].type == constant_dof)
            {
               strcat(prescribed_string," 1");
               sprintf(buffer,"fixed%d", *dofcount);
               mstrcpy(&jnt->dofs[ax].sd.name,buffer);
               jnt->dofs[ax].sd.state_number = (*dofcount)++;
               jnt->dofs[ax].sd.constrained = no;
               jnt->dofs[ax].sd.fixed = yes;
               jnt->dofs[ax].sd.conversion = RTOD*jnt->dofs[ax].sd.conversion_sign;
               jnt->dofs[ax].sd.joint = sdnum;//jointnum;
               jnt->dofs[ax].sd.axis = axisnum++;
               jnt->dofs[ax].sd.initial_value = jnt->dofs[ax].value;
            }
            else
            {
               jnt->dofs[ax].sd.state_number = (*dofcount)++;
               jnt->dofs[ax].sd.fixed = no;
               if (jnt->dofs[ax].sd.constrained == yes)
               {
                  jnt->dofs[ax].sd.error_number = (*constrainedcount)++;
                  strcat(prescribed_string," 0");
               }
               else
                  strcat(prescribed_string," 0?");
               jnt->dofs[ax].sd.conversion = RTOD*jnt->dofs[ax].sd.conversion_sign;
               jnt->dofs[ax].sd.joint = sdnum;//jointnum;
               jnt->dofs[ax].sd.axis = axisnum++;
               if (jnt->dofs[ax].sd.constrained == yes)
                  jnt->dofs[ax].sd.initial_value = jnt->dofs[ax].value;
               else
                  jnt->dofs[ax].sd.initial_value = jnt->dofs[ax].gencoord->value;
            }
         }
      }
      if (write_file == yes)
         fprintf(fp, "prescribed =%s\n", prescribed_string);
   }

   if (write_file == yes)
      fprintf(fp, "\n");

   /* Now that you're done making the SD/FAST joint...
    * If the joint in the SD/FAST code is the reverse direction of the
    * joint in SIMM, change its type on the SIMM side so that the code
    * that checks for model consistency does not complain. This is done
    * only for planar and bushing joints (the others are not implemented
    * as reverse joints in SD/FAST).
    */
   if (jntsdf->dir == FORWARD)
   {
#if 0 // JPL 12/2/08: nothing should be done for FORWARD, right?
      if (jnt->type == dpReversePlanar)
         jnt->type = dpPlanar;
      else if (jnt->type == dpReverseBushing)
         jnt->type = dpBushing;
#endif
   }
   else
   {
      if (jnt->type == dpPlanar)
         jnt->type = dpReversePlanar;
      else if (jnt->type == dpBushing)
         jnt->type = dpReverseBushing;
   }
}


void static write_loop_joints(FILE* fp, char seg_name[], SegmentSDF* seg, int addQuestionMarks)
{
   int i;
   char* new_name;

   for (i=0; i<seg->times_split; i++)
   {
      new_name = make_sdfast_seg_name(seg_name, i+1);
      fprintf(fp, "body = %s inb = %s joint = weld\n", seg_name, new_name);
      fprintf(fp, "bodytojoint = 0.0 0.0 0.0   inbtojoint = 0.0 0.0 0.0\n\n");
      FREE_IFNOTNULL(new_name);
   }
}


/* VALID_SDFAST_MODEL: this routine checks the model to make sure that the
 * current implementation of SIMM can correctly represent it with an SD/FAST
 * input file.
 */

SBoolean valid_sdfast_model(ModelStruct* ms)
{
   int i, jointnum, dofnum;

   for (i = 0; i < ms->numjoints; i++)
   {
      if (ms->joint[i].type != dpPlanar &&
          ms->joint[i].type != dpReversePlanar &&
          ms->joint[i].type != dpPin &&
          ms->joint[i].type != dpReversePin &&
          ms->joint[i].type != dpUniversal &&
          ms->joint[i].type != dpReverseUniversal &&
          ms->joint[i].type != dpWeld &&
          ms->joint[i].type != dpSlider &&
          ms->joint[i].type != dpCylindrical &&
          ms->joint[i].type != dpReverseCylindrical &&
          ms->joint[i].type != dpGimbal &&
          ms->joint[i].type != dpReverseGimbal &&
          ms->joint[i].type != dpBushing &&
          ms->joint[i].type != dpReverseBushing &&
          ms->joint[i].type != dpSkippable)
      {
          sprintf(errorbuffer, "Joint %s cannot be converted to an SD/FAST joint.", ms->joint[i].name);
         return no;
      }
   }

   for (i = 0; i < ms->numgencoords; i++)
   {
      if (ms->gencoord[i]->used_in_model == yes)
      {
          if (!mark_unconstrained_dof(ms, ms->gencoord[i], &jointnum, &dofnum, NULL))
         {
             sprintf(errorbuffer, "At least one DOF must be a \"simple\" function of gencoord %s (2 points, slope=1, passes thru zero).",
               ms->gencoord[i]->name);
            return no;
         }
      }
   }

   return yes;
}


void write_forparams(char filename[], ModelStruct* model)
{
   int i, j, nq;
   FILE* file;
   DofStruct* dof;
   char buf[CHARBUFFER];
   char fullpath[CHARBUFFER];
   char* time_string;

   file = simm_fopen(filename, "w");

   if (file == NULL)
      return;

   if (model->musclefilename)
   {
      if (is_absolute_path(model->musclefilename))
      {
         strcpy(buf, model->musclefilename);
      }
      else
      {
         char* p;

         strcpy(buf, model->jointfilename);
         p = strrchr(buf, DIR_SEP_CHAR);

         if (p)
            p++;
         else
            p = buf;
         strcpy(p, model->musclefilename);
      }
   }
   make_time_string(&time_string);

   fprintf(file, "# PARAMS.TXT\n");
   fprintf(file, "# Dynamics Pipeline parameters file generated by %s\n", program_with_version);
   fprintf(file, "# for use with the stand-alone version of a dynamic simulation.\n");
   fprintf(file, "# Created %s\n", time_string);
   fprintf(file, "# Name of original SIMM joints file: %s\n", model->jointfilename);
   fprintf(file, "#\n");
   fprintf(file, "# This file contains all of the parameters that you can specify at\n");
   fprintf(file, "# run-time in a dynamic simulation (stand-alone version). To change\n");
   fprintf(file, "# one of them, remove the '#' from the beginning of the line (which\n");
   fprintf(file, "# identifies the line as a comment), and change the parameter to the\n");
   fprintf(file, "# desired value.\n\n");

   fprintf(file, "############################# I/O Options #############################\n");
   if (model->musclefilename)
      fprintf(file, "muscle_file %s\n", buf);
   else
      fprintf(file, "#muscle_file model.msl\n");
   if (model->bonepathname)
   {
      if (model->jointfilename)
      {
         char* purePath = NULL;
         get_pure_path_from_path(model->jointfilename, &purePath);
         build_full_path(purePath, model->bonepathname, fullpath);
         FREE_IFNOTNULL(purePath);
      }
      else
      {
         build_full_path(NULL, model->bonepathname, fullpath);
      }
      fprintf(file, "bone_path %s\n", fullpath);
   }
   else
   {
      fprintf(file, "#bone_path path_to_bone_files\n");
   }
   fprintf(file, "output_motion_file results.mot\n");
   fprintf(file, "#kinetics_file forces.ktx\n");
   fprintf(file, "output_kinetics_file results.ktx\n\n");

   fprintf(file, "######################## Integration Parameters #######################\n");
   fprintf(file, "start_time 0.0  #start time of simulation\n");
   fprintf(file, "end_time   1.0  #end time of simulation\n");
   fprintf(file, "step_size  0.01 #reporting interval for integrator\n\n");

   fprintf(file, "######################### Segment Parameters ##########################\n");
   fprintf(file, "# You can specify mass properties here if you add '?' after their\n");
   fprintf(file, "# values in model.sd. Mass centers should only be changed by editing\n");
   fprintf(file, "# your SIMM joint file and re-saving the dynamics files.\n");
   fprintf(file, "# segment_name mass value\n");
   fprintf(file, "# segment_name inertia I11 I12 I13 I21 I22 I23 I31 I32 I33\n");
   for (i=1; i<num_SD_segs; i++)
   {
      fprintf(file, "#%s mass %20.16lf\n", SDseg[i].name, SDseg[i].mass);
      fprintf(file, "#%s inertia %.16lf %.16lf %.16lf %.16lf %.16lf %.16lf %.16lf %.16lf %.16lf\n", SDseg[i].name, SDseg[i].inertia[0][0],
         SDseg[i].inertia[0][1], SDseg[i].inertia[0][2], SDseg[i].inertia[1][0],
         SDseg[i].inertia[1][1], SDseg[i].inertia[1][2], SDseg[i].inertia[2][0],
         SDseg[i].inertia[2][1], SDseg[i].inertia[2][2]);
   }
   fprintf(file, "\n");

   fprintf(file, "####################### Gencoord Initialization #######################\n");
   fprintf(file, "# gencoord_name initial_value initial_velocity\n");
   fprintf(file, "#\n");
   for (i=0, nq=0; i<model->numjoints; i++)
      for (j=0; j<6; j++)
         if (model->joint[i].dofs[j].type == function_dof ||
            model->joint[i].dofs[j].sd.fixed == yes)
            nq++;

   for (i=0; i<nq; i++)
   {
      dof = find_nth_q_dof(model, i);
      if (dof != NULL)
         fprintf(file, "#%s %lf 0.0\n", dof->sd.name, dof->sd.initial_value);
   }
   fprintf(file, "\n");

   fprintf(file, "########################### Output Options ############################\n");
   fprintf(file, "# this one is for output printed to the shell window\n");
   fprintf(file, "verbose no\n");
   fprintf(file, "# these are for output written to output_motion_file\n");
   fprintf(file, "output_gencoord_values yes\n");
   fprintf(file, "output_muscle_activations yes\n");
   fprintf(file, "output_muscle_lengths no\n");
   fprintf(file, "output_muscle_forces yes\n");
   fprintf(file, "output_muscle_fiber_lengths no\n");
   fprintf(file, "output_muscle_fiber_velocities no\n");
   fprintf(file, "output_muscle_moment_arms no\n");
   fprintf(file, "output_muscle_joint_torques no\n");
   fprintf(file, "output_total_muscle_joint_torques no\n");
   fprintf(file, "output_joint_torques no\n");
   fprintf(file, "output_corrected_joint_torques no\n");
   fprintf(file, "output_joint_reaction_forces no\n");
   fprintf(file, "output_joint_reaction_torques no\n");
   fprintf(file, "output_optimized_muscle_activations no\n");
   fprintf(file, "output_mass_center_positions no\n");
   fprintf(file, "output_mass_center_velocities no\n");
   fprintf(file, "output_system_energy no\n");
   fprintf(file, "output_contact_forces no\n");
   fprintf(file, "output_spring_forces no\n");
   fprintf(file, "num_output_spring_forces 10\n");
   fprintf(file, "\n");

   fprintf(file, "####################### Force Matte Definitions #######################\n");
   fprintf(file, "# force mattes can be used to apply forces defined in the ground\n");
   fprintf(file, "# reference frame to some other body segment.\n");
   fprintf(file, "# force_matte matte_name bone_file_name segment_name\n");
   for (i = 0; i < model->numsegments; i++)
   {
      if (model->segment[i].forceMatte)
      {
         fprintf(file,"force_matte %s %s\t%s\n", model->segment[i].forceMatte->name,
            model->segment[i].forceMatte->filename, model->segment[i].name);
      }
   }
   fprintf(file, "\n");

   fprintf(file, "##################### Contact Detection Variables #####################\n");
   fprintf(file, "# object object_name bone_file_name segment_name\n");
   fprintf(file, "# begin_group group_name object_name1 ... object_nameN end_group\n");
   fprintf(file, "# contact_pair object_name1 object_name2 coef_rest mu_dynamic mu_static\n\n");
   for (i=0; i<model->numsegments; i++)
   {
      for (j=0; j<model->segment[i].numContactObjects; j++)
      {
         fprintf(file,"object %s %s\t%s\n", model->segment[i].contactObject[j].name,
            model->segment[i].contactObject[j].filename, model->segment[i].name);
      }
   }
   for (i=0; i<model->numContactGroups; i++)
   {
      fprintf(file,"begin_group %s\n", model->contactGroup[i].name);
      for (j=0; j<model->contactGroup[i].numElements; j++)
         fprintf(file,"%s ", model->contactGroup[i].element[j]);
      fprintf(file,"\nend_group\n");
   }
   for (i=0; i<model->numContactPairs; i++)
   {
      fprintf(file,"contact_pair %s %s %lf %lf %lf\n",
         model->contactPair[i].body1,
         model->contactPair[i].body2,
         model->contactPair[i].restitution,
         model->contactPair[i].mu_static,
         model->contactPair[i].mu_dynamic);
   }

   fprintf(file, "######################## Spring-based Contacts ########################\n");
   fprintf(file, "# spring_floor spring_floor_name file_name segment_name\n");
   fprintf(file, "# spring segment_name Px Py Pz spring_floor_name friction param1 param2 param3 param4 param5 param6\n");
   for (i=0; i<model->numsegments; i++)
   {
      if (model->segment[i].springFloor)
      {
         fprintf(file,"spring_floor %s %s\t%s\n", 
            model->segment[i].springFloor->name, 
            model->segment[i].springFloor->filename,
            model->segment[i].name);
      }
      for (j=0; j<model->segment[i].numSpringPoints; j++)
      {
         fprintf(file,"spring %s % 10.4f % 10.4f % 10.4f "
            "%s % 10.4f % 12.4f % 12.4f % 12.4f % 12.4f % 12.4f % 12.4f\n", 
            model->segment[i].name,
            model->segment[i].springPoint[j].point[0],
            model->segment[i].springPoint[j].point[1],
            model->segment[i].springPoint[j].point[2],
                model->segment[model->segment[i].springPoint[j].floorSegment].springFloor->name,
            model->segment[i].springPoint[j].friction,
            model->segment[i].springPoint[j].param_a,
            model->segment[i].springPoint[j].param_b,
            model->segment[i].springPoint[j].param_c,
            model->segment[i].springPoint[j].param_d,
            model->segment[i].springPoint[j].param_e,
            model->segment[i].springPoint[j].param_f);
      }
   }
   fprintf(file, "\n");

   fprintf(file, "##################### Muscle State Initialization #####################\n");
   fprintf(file, "# You can specify the initial values of the muscle model states for each\n");
   fprintf(file, "# muscle by un-commenting the appropriate line[s] below and replacing\n");
   fprintf(file, "# the zeros with the initial values. You must specify the state values\n");
   fprintf(file, "# in the order that the states are numbered in the muscle derivative\n");
   fprintf(file, "# function for that muscle model (in derivs.c). You can specify fewer\n");
   fprintf(file, "# than the total number of states in the muscle model, but the N values\n");
   fprintf(file, "# that you specify will be used to initialize the *first* N states in the\n");
   fprintf(file, "# model. If you specify more states than there are in the muscle model,\n");
   fprintf(file, "# the extra values will be ignored.\n");
   fprintf(file, "# muscle_name state1_init_value state2_init_value ...\n");
   fprintf(file, "#\n");
   for (i = 0; i < model->nummuscles; i++)
      fprintf(file, "#%s 0.0 0.0 0.0\n", model->muscle[i]->name);
   fprintf(file, "\n");

   fprintf(file, "##################### Wrap Object Initialization #####################\n");
   fprintf(file, "# You can specify the parameters of any of the wrap objects, in case\n");
   fprintf(file, "# you want to change them from their values when sdfor.c was created.\n");
   fprintf(file, "# If you specify a wrap object's parameters here, you must specify\n");
   fprintf(file, "# all of its parameters, and you must use the following format:\n");
   fprintf(file, "#\n");
   fprintf(file, "# name type active algorithm segment dim1 dim2 dim3 quadrant tx ty tz rx ry rz\n");
   fprintf(file, "#\n");
   fprintf(file, "# Explanation of parameters:\n");
   fprintf(file, "# name: the name of the wrap object, as defined in the model when sdfor.c\n");
   fprintf(file, "#       was created.\n");
   fprintf(file, "# type: specify \"sphere\", \"ellipsoid\", \"cylinder\", or \"torus\".\n");
   fprintf(file, "# active: is the wrap object active? specify \"1\" for yes, \"0\" for no.\n");
   fprintf(file, "# algorithm: default algorithm to use for ellipsoid objects, in case\n");
   fprintf(file, "#            algorithm is not specified in the muscle definition. Specify\n");
   fprintf(file, "#            \"hybrid\", \"midpoint\", or \"axial\". For sphere, cylinder, and\n");
   fprintf(file, "#            torus objects, specify \"0\".\n");
   fprintf(file, "# segment: the name of the body segment that the object is attached to.\n");
   fprintf(file, "# dim1, dim2, dim3: for spheres, specify \"<radius> 0.0 0.0\"\n");
   fprintf(file, "#                   for ellipsoids, specify \"<x_radius> <y_radius> <z_radius>\"\n");
   fprintf(file, "#                   for cylinders, specify \"<radius> <height> 0.0\"\n");
   fprintf(file, "#                   for torii, specify \"<inner_radius> <outer_radius> 0.0\"\n");
   fprintf(file, "# quadrant: to constrain wrapping over half of the wrap object, define the\n");
   fprintf(file, "#           active region by specifying \"x\", \"-x\", \"y\", \"-y\", \"z\", or \"-z\".\n");
   fprintf(file, "#           For unconstrained wrapping, specify \"all\".\n");
   fprintf(file, "# tx, ty, tz: the location of the wrap object's origin in the body segment's\n");
   fprintf(file, "#             reference frame.\n");
   fprintf(file, "# rx, ry, rz: the XYZ Euler angles (body-fixed) expressing the orientation of\n");
   fprintf(file, "#             the wrap object in the body segment's reference frame.\n");
   fprintf(file, "#\n");
   for (i = 0; i < model->num_wrap_objects; i++)
   {
      double xyz[3], radius[] = {0.0, 0.0, 0.0};
      dpWrapObject* wo = model->wrapobj[i];
      const char *wrap_quadrant = "all";

      strcpy(buffer, wo->name);
      convert_string(buffer, yes);

      recalc_xforms(wo);
      extract_xyz_rot_bodyfixed(wo->from_local_xform, xyz);

      memcpy(radius, wo->radius, 3*sizeof(double));
      if (wo->wrap_type == dpWrapSphere) {
         radius[1] = radius[2] = 0.0;
      } else if (wo->wrap_type == dpWrapCylinder) {
         radius[1] = wo->height;
         radius[2] = 0.0;
      } else if (wo->wrap_type == dpWrapTorus) {
         radius[2] = 0.0;
      }

      if (wo->wrap_sign != 0)
      {
         switch ((wo->wrap_axis + 1) * wo->wrap_sign)
         {
            default: break;
            case  1: wrap_quadrant =  "x"; break;
            case -1: wrap_quadrant = "-x"; break;
            case  2: wrap_quadrant =  "y"; break;
            case -2: wrap_quadrant = "-y"; break;
            case  3: wrap_quadrant =  "z"; break;
            case -3: wrap_quadrant = "-z"; break;
         }
      }

      fprintf(file,"#%s %s %d %s %s %.10lf %.10lf %.10lf %s %.10lf %.10lf %.10lf %.4lf %.4lf %.4lf\n",
         buffer,
         get_wrap_type_name(wo->wrap_type),
         (wo->active == yes) ? 1 : 0,
         (wo->wrap_type == dpWrapEllipsoid) ? (char*)get_wrap_algorithm_name(wo->wrap_algorithm) : "0",
         SDseg[get_sd_seg_num(model->segment[wo->segment].name)+1].name,
         radius[0], radius[1], radius[2],
         wrap_quadrant,
         wo->translation.xyz[0], wo->translation.xyz[1], wo->translation.xyz[2],
         xyz[0]*RTOD, xyz[1]*RTOD, xyz[2]*RTOD);
   }

   fclose(file);

   sprintf(buffer, "Created: %s.", filename);
   message(buffer, 0, DEFAULT_MESSAGE_X_OFFSET);   
}
         

void write_dllparams(char filename[], ModelStruct* model)
{
   int i, j, nq;
   FILE* file;
   DofStruct* dof;
   char* time_string;

   file = simm_fopen(filename, "w");

   if (file == NULL)
      return;

   make_time_string(&time_string);

   fprintf(file, "# DLLPARAMS.TXT\n");
   fprintf(file, "# Dynamics Pipeline parameters file generated by %s\n", program_with_version);
   fprintf(file, "# for use with the DLL version of a dynamic simulation.\n");
   fprintf(file, "# Created %s\n", time_string);
   fprintf(file, "# Name of original SIMM joints file: %s\n", model->jointfilename);
   fprintf(file, "#\n");
   fprintf(file, "# This file contains all of the parameters that you can specify at\n");
   fprintf(file, "# run-time in a dynamic simulation (DLL version). To change one\n");
   fprintf(file, "# of them, remove the '#' from the beginning of the line (which\n");
   fprintf(file, "# identifies the line as a comment), and change the parameter to the\n");
   fprintf(file, "# desired value. This file is read by the simulation when you press\n");
   fprintf(file, "# the \"initialize\" button in the Dynamics Tool.\n\n");

   fprintf(file, "####################### Gencoord Initialization #######################\n");
   fprintf(file, "# gencoord_name initial_value initial_velocity\n");
   fprintf(file, "#\n");
   for (i=0, nq=0; i<model->numjoints; i++)
      for (j=0; j<6; j++)
         if (model->joint[i].dofs[j].type == function_dof ||
            model->joint[i].dofs[j].sd.fixed == yes)
            nq++;

   for (i=0; i<nq; i++)
   {
      dof = find_nth_q_dof(model, i);
      if (dof != NULL)
         fprintf(file, "#%s %lf 0.0\n", dof->sd.name, dof->sd.initial_value);
   }
   fprintf(file, "\n");

   fprintf(file, "##################### Muscle State Initialization #####################\n");
   fprintf(file, "# You can specify the initial values of the muscle model states for each\n");
   fprintf(file, "# muscle by un-commenting the appropriate line[s] below and replacing\n");
   fprintf(file, "# the zeros with the initial values. You must specify the state values\n");
   fprintf(file, "# in the order that the states are numbered in the muscle derivative\n");
   fprintf(file, "# function for that muscle model (in derivs.c). You can specify fewer\n");
   fprintf(file, "# than the total number of states in the muscle model, but the N values\n");
   fprintf(file, "# that you specify will be used to initialize the *first* N states in the\n");
   fprintf(file, "# model. If you specify more states than there are in the muscle model,\n");
   fprintf(file, "# the extra values will be ignored.\n");
   fprintf(file, "# muscle_name state1_init_value state2_init_value ...\n");
   fprintf(file, "#\n");
   for (i = 0; i < model->nummuscles; i++)
      fprintf(file, "#%s 0.0 0.0 0.0\n", model->muscle[i]->name);
   fprintf(file, "\n");

   fclose(file);

   sprintf(buffer, "Created: %s.", filename);
   message(buffer, 0, DEFAULT_MESSAGE_X_OFFSET);   
}


void write_dllcontactparams(char filename[], ModelStruct* model)
{
   int i, j, nq;
   FILE* file;
   char bonepath[CHARBUFFER];
   char* time_string;

   file = simm_fopen(filename, "w");

   if (file == NULL)
      return;

   make_time_string(&time_string);

   /* Make a string to hold the bone path so it can be added to
    * each contact object file name.
    */
   if (model->bonepathname)
   {
      if (model->jointfilename)
      {
         char* purePath = NULL;
         get_pure_path_from_path(model->jointfilename, &purePath);
         build_full_path(purePath, model->bonepathname, bonepath);
         FREE_IFNOTNULL(purePath);
      }
      else
      {
         build_full_path(NULL, model->bonepathname, bonepath);
      }
   }
   else
   {
      strcpy(bonepath, ".");
   }

   fprintf(file, "# DLLCONTACTPARAMS.TXT\n");
   fprintf(file, "# Dynamics Pipeline contact parameters file generated by %s\n", program_with_version);
   fprintf(file, "# for use with the DLL version of a dynamic simulation.\n");
   fprintf(file, "# Created %s\n", time_string);
   fprintf(file, "# Name of original SIMM joints file: %s\n", model->jointfilename);
   fprintf(file, "#\n");
   fprintf(file, "# This file contains all of the parameters needed for rigid-body contact\n");
   fprintf(file, "# detection (the parameters specified between the keywords \"begincontact\"\n");
   fprintf(file, "# and \"endcontact\" in the joint file). They are written to this file\n");
   fprintf(file, "# every time the \"initialize\" button is pressed in the Dynamics Tool\n");
   fprintf(file, "# because they are not included in the dpModelStruct object that is passed\n");
   fprintf(file, "# to the DLL at that time.\n\n");

   fprintf(file, "##################### Contact Detection Variables #####################\n");
   for (i=0; i<model->numsegments; i++)
   {
      for (j=0; j<model->segment[i].numContactObjects; j++)
      {
         fprintf(file,"object %s %s\\%s\t%s\n", model->segment[i].contactObject[j].name,
            bonepath, model->segment[i].contactObject[j].filename, model->segment[i].name);
      }
   }
   for (i=0; i<model->numContactGroups; i++)
   {
      fprintf(file,"begin_group %s\n", model->contactGroup[i].name);
      for (j=0; j<model->contactGroup[i].numElements; j++)
         fprintf(file,"%s ", model->contactGroup[i].element[j]);
      fprintf(file,"\nend_group\n");
   }
   for (i=0; i<model->numContactPairs; i++)
   {
      fprintf(file,"contact_pair %s %s %lf %lf %lf\n",
         model->contactPair[i].body1,
         model->contactPair[i].body2,
         model->contactPair[i].restitution,
         model->contactPair[i].mu_static,
         model->contactPair[i].mu_dynamic);
   }

   fclose(file);

   sprintf(buffer, "Created: %s.", filename);
   message(buffer, 0, DEFAULT_MESSAGE_X_OFFSET);   
}
         
